#include "agv_util/point_util.hpp"
#include "yaml-cpp/yaml.h"
#include <fstream>

namespace agv_util
{

    geometry_msgs::msg::PoseStamped point_to_pose(std::string pointName)
    {
        geometry_msgs::msg::PoseStamped poseStamped;
        poseStamped.header.frame_id = "error";
        // 读取 JSON 文件
        std::ifstream file("/home/fishros/ws_git/ws_ros2/ws_ros2_study/src/agv_util/mock/pose.json");
        if (!file.is_open())
        {
            std::cerr << "Failed to open pose.json" << std::endl;
            return poseStamped; // 返回默认构造的对象
        }

        // 解析 JSON 数据
        json jsonData;
        try
        {
            file >> jsonData;
        }
        catch (const json::parse_error &e)
        {
            std::cerr << "JSON parse error: " << e.what() << std::endl;
            return poseStamped; // 返回默认构造的对象
        }

        // 查找 pointName 对应的 pose
        for (const auto &item : jsonData)
        {
            if (item["pointName"] == pointName)
            {
                poseStamped.header.frame_id = "map";
                poseStamped.pose.position.x = item["pose"]["position"]["x"];
                poseStamped.pose.position.y = item["pose"]["position"]["y"];
                poseStamped.pose.orientation.w = item["pose"]["orientation"]["w"];
                // 如果有其他方向信息，继续添加
                break;
            }
        }

        return poseStamped;
    }

    std::vector<nav_msg_pkg::msg::Waypoint> load_waypoint()
    {
        rclcpp::Logger logger_ = rclcpp::get_logger("agv_util");
        try
        {
            std::vector<nav_msg_pkg::msg::Waypoint> waypoints;
            std::string home_dir = getenv("HOME");
            std::string file_path = home_dir + "/waypoints.yaml";
            std::ifstream infile(file_path);
            if (!infile)
            {
                RCLCPP_ERROR(logger_, "无法打开文件 %s", file_path.c_str());
                return {};
            }

            YAML::Node config = YAML::Load(infile);
            int points_num = config["Points_Num"].as<int>();

            for (int i = 0; i < points_num; i++)
            {
                std::string waypointKey = "Point_" + std::to_string(i + 1);
                YAML::Node pointNode = config[waypointKey];
                nav_msg_pkg::msg::Waypoint waypoint;
                waypoint.frame_id = pointNode["Frame_id"].as<std::string>();
                waypoint.name = pointNode["Name"].as<std::string>();
                waypoint.type = pointNode["Type"].as<std::string>();
                waypoint.pose.position.x = pointNode["Pos_x"].as<double>();
                waypoint.pose.position.y = pointNode["Pos_y"].as<double>();
                waypoint.pose.position.z = pointNode["Pos_z"].as<double>();
                waypoint.pose.orientation.x = pointNode["Ori_x"].as<double>();
                waypoint.pose.orientation.y = pointNode["Ori_y"].as<double>();
                waypoint.pose.orientation.z = pointNode["Ori_z"].as<double>();
                waypoint.pose.orientation.w = pointNode["Ori_w"].as<double>();
                waypoints.push_back(waypoint);
            }
            RCLCPP_INFO(logger_, "已加载 points.size=%zu", waypoints.size());
            return waypoints;
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR(logger_, "加载点数据时发生错误: %s", e.what());
            return {};
        }
    }

    bool save_waypoint(std::vector<nav_msg_pkg::msg::Waypoint> &waypoints)
    {
        rclcpp::Logger logger_ = rclcpp::get_logger("agv_util");
        try
        {
            YAML::Node content;

            int size_points = waypoints.size();
            content["Points_Num"] = size_points;
            for (int i = 0; i < size_points; i++)
            {
                YAML::Node node;
                node["Type"] = "Point";
                node["Name"] = "point_" + std::to_string(i + 1);
                node["Frame_id"] = waypoints[i].frame_id;
                node["Pos_x"] = std::round(waypoints[i].pose.position.x * 1000.0) / 1000.0;
                node["Pos_y"] = std::round(waypoints[i].pose.position.y * 1000.0) / 1000.0;
                node["Pos_z"] = std::round(waypoints[i].pose.position.z * 1000.0) / 1000.0;
                node["Ori_x"] = std::round(waypoints[i].pose.orientation.x * 1000.0) / 1000.0;
                node["Ori_y"] = std::round(waypoints[i].pose.orientation.y * 1000.0) / 1000.0;
                node["Ori_z"] = std::round(waypoints[i].pose.orientation.z * 1000.0) / 1000.0;
                node["Ori_w"] = std::round(waypoints[i].pose.orientation.w * 1000.0) / 1000.0;
                std::string waypointKey = "Point_" + std::to_string(i + 1);
                content[waypointKey] = node;
            }
            std::string home_dir = getenv("HOME");
            std::string file_path = home_dir + "/waypoints.yaml";
            std::ofstream outfile(file_path);
            if (!outfile)
            {
                throw std::runtime_error("无法打开文件 " + file_path);
                return false;
            }

            outfile << content;
            outfile.close();

            RCLCPP_INFO(logger_, "点数据已保存到 points.yaml");
            return true;
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR(logger_, "保存点数据时发生错误: %s", e.what());
            return false;
        }
    }

    std::vector<WaypointNeighbors> load_waypoint_neighbors()
    {
        std::vector<WaypointNeighbors> waypoint_neighbors;
        try
        {
            // 读取 JSON 文件
            std::ifstream file("/home/fishros/ws_git/ws_ros2/ws_ros2_study/src/agv_util/mock/waypoint_neighbors.json");
            if (!file.is_open())
            {
                std::cerr << "Failed to open pose.json" << std::endl;
                return waypoint_neighbors;
            }

            // 解析 JSON 数据
            json jsonData;
            // 解析JSON数据
            file >> jsonData;

            // 遍历JSON数组并填充点向量
            for (const auto &point_json : jsonData)
            {
                WaypointNeighbors waypoint_neighbor;
                waypoint_neighbor.name = point_json["name"];
                waypoint_neighbor.neighbors = point_json["neighbors"].get<std::vector<std::string>>();
                waypoint_neighbors.push_back(waypoint_neighbor);
            }
        }
        catch (json::exception &e)
        {
            std::cerr << "JSON parse error: " << e.what() << std::endl;
        }
        return waypoint_neighbors;
    }
}